#include <iostream>
#include <algorithm>
#include <math.h>

//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/io.hpp>
//#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/vector_proxy.hpp>
//#include <boost/numeric/ublas/matrix_proxy.hpp>
//#include <boost/numeric/ublas/triangular.hpp>
//#include <boost/numeric/ublas/lu.hpp>

#include <armadillo>
// g++ -std=c++11 karmarkar.cpp -O1 -larmadillo

//using namespace boost::numeric::ublas;
using namespace arma;
using namespace std;
/*
struct Program { 
    bool max; //true = maximize, false = minimize
    vector<type> objective; //i.e. C
    vector<vector<double>> constraints;    //i.e. A
    vector<string> inequalitySigns;    //contains the inequality operator within the subject equation, i.e. <, >, <=, >=, =
    vector<double> inequalityVals;  //i.e. b
    vector<string> constrainedVars;
    type b0;
};*/


struct karmar_type
{	mat C;
	mat A;
	karmar_type(int C_row, int C_col, int  A_row, int A_col)
	{
		C.resize(C_row,C_col);
		A.resize(A_row,A_col);
		C=zeros(C_row,C_col);
		A=zeros(A_row,A_col);
	}
	~karmar_type()
	{
		//C.clear();
		//A.clear();
		//e.clear();
		//delete[] *C;
	} 

}; 

mat improvement(mat &x_k, mat &c, mat &A, mat &e)
{
	//input arguments x_k,c,A,e
	//%c=[2; 2; -3];  %given
	//%A=[-1 -2 3];   %given
	//%e=[1; 1; 1];   %given
	
			
	double n=e.n_rows;
	double alpha=(n-1)/(3*n);
	double r=1/sqrt(n*(n-1));
	mat X_k=diagmat(x_k);	//X_k and x_k are different
	mat B=join_cols(A*X_k,trans(e));		
	mat temp=trans(B)*inv(B*trans(B))*B;
	mat temp2(temp.n_rows,temp.n_cols);
	temp2.eye();
	mat P_B=temp2-temp;
	mat P=P_B+(1/n)*e*trans(e);
	mat delta_x=(-1)*P_B*X_k*c;
	mat y_k1=(e/n)+alpha*(1/sqrt(n*(n-1)))*delta_x/norm(delta_x,2);
	x_k=(X_k*y_k1)/as_scalar(trans(e)*X_k*y_k1);
	//cout<<x_k<<endl;
	return x_k;
}

karmar_type kar_form(mat &C, mat &A, mat &b, double magic)
{
	mat C_dual=trans(b);
	mat A_dual=trans(A);
	mat b_dual=trans(C);
	double rows=A_dual.n_rows;
	double cols=A_dual.n_cols;
	double r=A.n_rows;
	double c=A.n_cols;
	mat slack_p(r,r);
	slack_p.eye();
	mat slack_d(rows,rows);
	slack_d.eye();
	slack_d=(-1)*slack_d;	
	A=join_rows(A,slack_p);
	A_dual=join_rows(A_dual,slack_d);
	cout<<A<<endl;
	cout<<A_dual<<endl;
	
	rows=A_dual.n_rows;
	cols=A_dual.n_cols;
	r=A.n_rows;
	c=A.n_cols;
	
	A=join_rows(A,zeros(r,cols));
	A_dual=join_rows(zeros(rows,c),A_dual);

	rows=A_dual.n_rows;
	cols=A_dual.n_cols;
	r=A.n_rows;
	c=A.n_cols;

	mat b_con=ones(1,cols+1);
	A=join_rows(A,zeros(r,1));
	A_dual=join_rows(A_dual,zeros(rows,1));

	mat b_b_con={magic};
	A=join_rows(A,-1*b);
	A_dual=join_rows(A_dual,-1*b_dual);
	b_con=join_rows(b_con,-1*b_b_con);


	double k=slack_p.n_cols;
	double m=b_con.n_cols;
	
	mat A_final=join_rows(C,zeros(1,k));
	A_final=join_rows(A_final,-1*C_dual);

	double v=A_final.n_cols;	
	A_final=join_rows(A_final,zeros(1,m-v));
	A_final=join_cols(A_final,A);
	A_final=join_cols(A_final,A_dual);
	A_final=join_cols(A_final,b_con);

	r=A_final.n_rows;

	mat extra_col=sum(A_final,1);	//1 indicates sum of each row
	//extra_col(r-1)=-1;
	A_final=join_rows(A_final,-1*extra_col);
	c=A_final.n_cols;
	mat obj=zeros(1,c);
	obj(c-1)=1;
	obj=trans(obj);
	mat e=ones(1,c);
	A_final=join_cols(A_final,e);

	karmar_type kar_struct(obj.n_rows,obj.n_cols,A_final.n_rows,A_final.n_cols);
	kar_struct.C=obj;
	kar_struct.A=A_final;
	
	//cout<<kar_struct.C<<endl;
	
	//cout<<kar_struct.A<<endl;
	
	//cout<<"kar_struct"<<kar_struct.e<<endl;
	//cout<<A_final<<endl;
	//cout<<obj<<endl;
	//cout<<e<<endl;
	return  kar_struct;
}

void solve(mat Cin, mat Ain, mat bin, double magic)
{
	/*mat A = {-1,-2,3};
	mat c = {2,2,-3};
	c.reshape(c.n_cols,1);
	mat e = {1,1,1};*/
	
        karmar_type first = kar_form(Cin,Ain,bin,magic);
	int f=first.A.n_rows-1;
	mat e=first.A.row(f);
	first.A.shed_row(f);
	e=trans(e);		
	double n=e.n_rows;
	mat x_0=e/n;
	//cout<<"x_0 "<<x_0<<endl;
	//cout<<"first.e"<<first.e<<endl;
	//cout<<first.C<<endl;
	//cout<<first.A<<endl;
	//cout<<first.e<<endl;
	
	mat temp=improvement(x_0,first.C,first.A,e);
	cout<<"temp "<<temp<<endl;	
	
	while(abs(as_scalar(trans(first.C)*temp))>.000001)
	{
		temp=improvement(temp,first.C,first.A,e);
		cout<<temp<<endl;
	}
	cout<<temp*(magic+1)<<endl;
	
	
}

int main()
{
	//karmarkar();
	//%C=[3 1];
	//%A=[2 -1;1 2];
	//%b=[2;5];

	//example 1	//magic_number>16
/*	mat C= {3,1};
	mat A;
	A<<2<<-1<<endr
	 <<1<<2<<endr;
	//cout<<A<<endl;
	mat b={2,5};
	b.reshape(2,1);

*/
	//example 2	//magic_number>200
	mat C= {7,12};
	mat A;
	A<<9<<4<<endr
	 <<4<<5<<endr
	 <<3<<10<<endr;
	//cout<<A<<endl;
	mat b={360,200,300};
	b.reshape(3,1);

	double magic_number; //high_estimate_of_solution*(number of variables+number of slack for both dual and primal)
	magic_number=2000; //need to find out how to make an estimate of this number
	solve(C,A,b,magic_number);
}
